import org.apache.commons.math.linear.Array2DRowRealMatrix;
import org.apache.commons.math.linear.RealMatrix;

import edu.wsu.KheperaSimulator.RobotController;

/**
 * 
 * The Controller3a class. The robots runs toward a light bulb and runs into it.
 * Proportional controller
 * 
 * @author Philipp Rusch und Markus Wetzel
 */
public class Controller3a extends RobotController {

	// Sensoren:
	// 0 .. Links
	// 1 .. Links Vorne Schraeg
	// 2 .. links Vorne
	// 3 .. Rechts Vorne
	// 4 .. Rechts Vorne Schraeg
	// 5 .. Rechts
	// 6 .. Rechts Hinten
	// 7 .. Links Hinten

	double[][] controllerMatrixData = {
			{ 0d, 0d, 0d, 0.2d, 0.2d, 0.2d, 0.2d, 0d },
			{ 0.2d, 0.2d, 0.2d, 0d, 0d, 0d, 0d, 0.2d } };
	RealMatrix controllerMatrix = new Array2DRowRealMatrix(controllerMatrixData);

	@Override
	public void close() throws Exception {
	}

	@Override
	public void doWork() throws Exception {
		double sumSensorValues = 0;
		for (int i = 0; i < 8; i++) {
			sumSensorValues += getLightValue(i);
		}

		System.out.println("sumSensorValues: " + sumSensorValues);
		System.out.println("Sensor 0: " + getLightValue(0));
		System.out.println("Matrix 0: " + 1 / getLightValue(0)
				* sumSensorValues);

		double[][] sensorVectorData = {
				{ 1 / (double) getLightValue(0) * sumSensorValues },
				{ 1 / (double) getLightValue(1) * sumSensorValues },
				{ 1 / (double) getLightValue(2) * sumSensorValues },
				{ 1 / (double) getLightValue(3) * sumSensorValues },
				{ 1 / (double) getLightValue(4) * sumSensorValues },
				{ 1 / (double) getLightValue(5) * sumSensorValues },
				{ 1 / (double) getLightValue(6) * sumSensorValues },
				{ 1 / (double) getLightValue(7) * sumSensorValues } };

		RealMatrix sensorVector = new Array2DRowRealMatrix(sensorVectorData);

		// Now multiply controllerMatrix by sensorMatrix
		RealMatrix motorVector = controllerMatrix.multiply(sensorVector);

		System.out.println(motorVector.getEntry(0, 0) + ","
				+ motorVector.getEntry(1, 0));

		setMotorSpeeds((int) motorVector.getEntry(0, 0),
				(int) motorVector.getEntry(1, 0));

	}
}
